#include "odom_calib.hpp"
#include <cmath>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

OdomCalib::OdomCalib() : Node("odom_calib")
{
	// Initialize publishers
	vel_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
			"/cmd_vel", 10);
	calib_pub = this->create_publisher<agv_msgs::msg::AgvOdomCalib>(
			"/agv_calib/odom_calib", 10);

	// Initialize subscribers
	odometry_sub = this->create_subscription<agv_msgs::msg::OdometryLite>(
			"/odom", 1,
			std::bind(&OdomCalib::OdomSubCallback, this, std::placeholders::_1));

	keyvel_sub = this->create_subscription<geometry_msgs::msg::TwistStamped>(
			"/cmd_vel", 1,
			std::bind(&OdomCalib::KeyvelSubCallback, this, std::placeholders::_1));

	// Initialize service
	odom_server = this->create_service<agv_srvs::srv::OdomCalib>(
			"/agv_calib/odom_calib",
			std::bind(&OdomCalib::OdomCalibCallback, this, 
				std::placeholders::_1, std::placeholders::_2));

	// Initialize parameters
	initparams();
}

float OdomCalib::Approach(float from, float to, float step)
{
	float target;

	if (from + step < to) {
		from += step;
	} else if (from - step > to) {
		from -= step;
	} else 	{
		from = to;
	}

	target = from;
	return target;
}

float OdomCalib::EstimateArea(float fStartValue, float fEndValue, float fSlope)
{
	// If the start value is equal to the end value, return 0
	if (fStartValue == fEndValue) return 0.0f;

	// Estimate the covering area
	return (float)(fabs((pow(fEndValue, 2) - pow(fStartValue, 2)) / (2 * fSlope)));
}

void OdomCalib::KeyvelSubCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
{
	current_vel.linear.x = msg->twist.linear.x;
	current_vel.angular.z = msg->twist.angular.z;
}

bool OdomCalib::initparams() {
	RCLCPP_INFO(this->get_logger(), "init odom calib params");
	shutdown_req = false;
	shutdown_vel = false;
	start_pose.position.x = 0;
	start_pose.position.y = 0;
	return true;
}

double OdomCalib::getdistance(geometry_msgs::msg::Pose cur, geometry_msgs::msg::Pose des){
	return sqrt(pow((cur.position.x - des.position.x), 2) + pow((cur.position.y - des.position.y), 2));
}

void OdomCalib::OdomSubCallback(const agv_msgs::msg::OdometryLite::SharedPtr msg)
{
	current_pose.position.x = msg->pose.position.x;
	current_pose.position.y = msg->pose.position.y;
	current_pose.orientation = msg->pose.orientation;
}

void OdomCalib::OdomCalibCallback(
		const std::shared_ptr<agv_srvs::srv::OdomCalib::Request> request,
		std::shared_ptr<agv_srvs::srv::OdomCalib::Response> response)
{
	if (request->start == 1) {   // start
	
		start = request->start;
		mode = request->model;
		target_vel = request->vel;
		oprange = request->oprange;

		RCLCPP_INFO(this->get_logger(), 
				"odom calib params start: %d mode: %d target_vel: %.2f oprange: %.2f",
				start, mode, target_vel, oprange);

		initparams();
	} else {    // end
		shutdown();
		response->status = "odom calib end";
		return;
	}

	calib_thread = std::thread(&OdomCalib::OdomCalibStart, this);
	calib_thread.detach();

	response->status = "odom calib start";
}

void OdomCalib::OdomCalibStart() {
	if (mode == 0) {    //distance
		RCLCPP_INFO(this->get_logger(), "odom straight calib start");
		Distance();
	}

	if (mode == 1) {    //spin
		RCLCPP_INFO(this->get_logger(), "odom spin calib start");
		Anglar();
	}
}

double OdomCalib::getRadDistance(double target, double current)
{
	return normalizationRad(current - target);
}

double OdomCalib::normalizationRad(double rad)
{
	double tmp = getRemainder(rad, 2 * M_PI);
	if (tmp > M_PI)
		return tmp - 2 * M_PI;
	if (tmp < -1 * M_PI)
		return tmp + 2 * M_PI;

	return tmp;
}

double OdomCalib::getRemainder(double x, double y)
{
	int n = trunc(x/y);
	return x - n*y;
}

bool OdomCalib::Distance() {
	rclcpp::Rate spin_rate(50);
	int delay = 0;
	float error_vx = 0;
	geometry_msgs::msg::TwistStamped vel_msg;
	vel_msg.header.stamp = this->now();
	vel_msg.header.frame_id = "calib_vel"; 
	vel_msg.twist.linear.x = 0.0;
	vel_msg.twist.linear.y = 0.0;
	vel_msg.twist.linear.z = 0.0;
	vel_msg.twist.angular.x = 0.0;
	vel_msg.twist.angular.y = 0.0;
	vel_msg.twist.angular.z = 0.0;

	agv_msgs::msg::AgvOdomCalib calib_msg;

	while (!shutdown_req && rclcpp::ok()) {
		if (!shutdown_vel && start == 1) {
			if(start_pose.position.y == 0.0 && start_pose.position.x == 0.0) {
				start_pose.position.y = current_pose.position.y;
				start_pose.position.x = current_pose.position.x;
				RCLCPP_INFO(this->get_logger(), 
						"Start test - dis:%.2f, cur pose:[%.4f, %.4f], vel:%.2f", 
						oprange, current_pose.position.x, current_pose.position.y, target_vel);
				delay = 10;
				calib_msg.process = 0;
			}

			double distance_err = oprange - fabs(getdistance(start_pose, current_pose));

			if (distance_err > 0.0 && distance_err < EstimateArea(current_vel.linear.x, 0.0, 0.06)) {
				vel_msg.twist.linear.x = Approach(current_vel.linear.x, 0.0, 0.02);
				calib_msg.process = fabs(getdistance(start_pose, current_pose)) / oprange * 100;
				RCLCPP_INFO(this->get_logger(), 
						"moving - target:%.2f, dis:[%.4f], vel:%.2f", 
						oprange, fabs(getdistance(start_pose, current_pose)), vel_msg.twist.linear.x);
				RCLCPP_INFO(this->get_logger(), "dcc vel:%.2f", vel_msg.twist.linear.x);
			} 
			else if (distance_err <= 0.0) {
				vel_msg.twist.linear.x = 0;
				vel_msg.twist.angular.z = 0;
				RCLCPP_INFO(this->get_logger(), 
						"set 0.0 vel, cur pose:[%.4f, %.4f]", 
						current_pose.position.x, current_pose.position.y);
				RCLCPP_INFO(this->get_logger(), 
						"set 0.0 vel, dis x:%.4f y:%.4f",
						fabs(current_pose.position.x - start_pose.position.x), 
						fabs(current_pose.position.y - start_pose.position.y));
				RCLCPP_INFO(this->get_logger(), 
						"set 0.0 vel, distance:%.4f",
						fabs(getdistance(start_pose, current_pose)));

				if (fabs(current_vel.linear.x) < 0.0003) {
					if (delay-- <= 0) {
						RCLCPP_INFO(this->get_logger(), 
								"End test - cur pose:[%.4f, %.4f], dis:[x:%.4f, y:%.4f]", 
								current_pose.position.x, current_pose.position.y,
								fabs(current_pose.position.x - start_pose.position.x), 
								fabs(current_pose.position.y - start_pose.position.y));
						RCLCPP_INFO(this->get_logger(), 
								"End test - distance:%.4f", 
								fabs(getdistance(start_pose, current_pose)));
						delay = 0;
						shutdown_vel = true;
						calib_msg.process = 100;
					}
				}
			} else {
				vel_msg.twist.linear.x = Approach(current_vel.linear.x, target_vel, 0.02);
				calib_msg.process = fabs(getdistance(start_pose, current_pose)) / oprange * 100;
				RCLCPP_INFO(this->get_logger(), 
						"moving - target:%.2f, dis:[%.4f], vel:%.2f", 
						oprange, fabs(getdistance(start_pose, current_pose)), vel_msg.twist.linear.x);
				RCLCPP_INFO(this->get_logger(), "acc vel:%.2f", vel_msg.twist.linear.x);
			}

			calib_msg.distance = fabs(getdistance(start_pose, current_pose));
			calib_msg.theta = 65535;

			// 更新时间戳
			vel_msg.header.stamp = this->now();
			RCLCPP_INFO(this->get_logger(), "pub vel:%.2f", vel_msg.twist.linear.x);
			vel_pub->publish(vel_msg);
		}

		calib_msg.header.stamp = this->now();
		calib_pub->publish(calib_msg);
		spin_rate.sleep();
	}
	return true;
}

bool OdomCalib::Anglar() {
	rclcpp::Rate spin_rate(50);
	int delay = 0, count = 0;
	double theta = 0;

	float diff_theta = 0;
	float last_yaw = 0;
	float sum_theta = 0;

	bool init_yaw = false;
	geometry_msgs::msg::TwistStamped vel_msg;
	vel_msg.header.stamp = this->now();
	vel_msg.header.frame_id = "calib_vel"; 
	vel_msg.twist.linear.x = 0.0;
	vel_msg.twist.linear.y = 0.0;
	vel_msg.twist.linear.z = 0.0;
	vel_msg.twist.angular.x = 0.0;
	vel_msg.twist.angular.y = 0.0;
	vel_msg.twist.angular.z = 0.0;

	agv_msgs::msg::AgvOdomCalib calib_msg;

	while (!shutdown_req && rclcpp::ok()) {
		if (!shutdown_vel && start == 1) {
			tf2::Quaternion quat;
			tf2::fromMsg(current_pose.orientation, quat);
			double roll, pitch, yaw;
			tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

			if (!init_yaw) {
				init_yaw = true;
				count = 0;
				theta = yaw + (target_vel > 0 ? oprange : oprange * -1) * M_PI / 180.0;
				if(theta >= M_PI) theta = theta - 2 * M_PI;
				if(theta <= -M_PI) theta = theta + 2 * M_PI;
				RCLCPP_INFO(this->get_logger(), "yaw:%.2f theta:%.2f", yaw, theta);
				RCLCPP_INFO(this->get_logger(), 
						"Start test - curret ang:%.2f, target ang:%.2f, ang vel:%.2f", 
						yaw * 180.0 / M_PI, theta * 180.0 / M_PI, target_vel);

				last_yaw = normalizationRad(yaw);
				calib_msg.process = 0;
			}

			sum_theta += abs(getRadDistance(last_yaw, yaw));
			last_yaw = yaw;

			double distance_err = oprange - sum_theta * 180 / M_PI;

			if (distance_err > 0.0 && distance_err * M_PI / 180 < EstimateArea(current_vel.angular.z, 0.0, 0.06)) {
				vel_msg.twist.angular.z = Approach(current_vel.angular.z, 0.0, 0.02);
				calib_msg.process = sum_theta * 180 / M_PI / oprange * 100;

				RCLCPP_INFO(this->get_logger(), "dcc vel:%.2f", vel_msg.twist.angular.z);
				RCLCPP_INFO(this->get_logger(), 
						"Rotating - ang:%.2f, diff:%.2f, ang vel:%.2f", 
						yaw * 180.0 / M_PI, distance_err, current_vel.angular.z);

			} else if (distance_err <= 0.0) {
				vel_msg.twist.linear.x = 0;
				vel_msg.twist.angular.z = 0;
				RCLCPP_INFO(this->get_logger(), 
						"set 0.0 vel - stop ang:%.2f diff:%.2f", 
						yaw * 180.0 / M_PI, distance_err);

				if (fabs(current_vel.angular.z) < 0.0003) {
					if (delay-- <= 0) {
						RCLCPP_INFO(this->get_logger(), 
								"End test - stop ang:%.2f, target ang:%.2f, diff:%.2f", 
								yaw * 180.0 / M_PI, theta * 180.0 / M_PI, distance_err);
						theta = 0;
						delay = 0;
						init_yaw = false;
						shutdown_vel = true;
						calib_msg.process = 100;
					}
				}
			} else {
				vel_msg.twist.angular.z = Approach(current_vel.angular.z, target_vel, 0.02);
				calib_msg.process = sum_theta * 180 / M_PI / oprange * 100;
				RCLCPP_INFO(this->get_logger(), "acc vel:%.2f", vel_msg.twist.angular.z);
				RCLCPP_INFO(this->get_logger(), 
						"Rotating - ang:%.2f, diff:%.2f, ang vel:%.2f", 
						yaw * 180.0 / M_PI, distance_err, current_vel.angular.z);
			}

			calib_msg.theta = sum_theta * 180 / M_PI;
			calib_msg.distance = 65535;

			vel_msg.header.stamp = this->now();
			RCLCPP_INFO(this->get_logger(), "pub vel:%.2f", vel_msg.twist.angular.z);
			vel_pub->publish(vel_msg);
		}

		calib_msg.header.stamp = this->now();
		calib_pub->publish(calib_msg);
		spin_rate.sleep();
	}
	return true;
}
